//
// = FILENAME
//    testSerializer.cc
//
// = AUTHOR(S)
//    Patric Jensfelt
//    
// = COPYRIGHT
//    Copyright (c) 2010 Patric Jensfelt
//
/*----------------------------------------------------------------------*/

#include "Serializer.hh"
#include <iostream>

int main()
{
  Serializer s;

  s.connect("/dev/ttyS2");

  /*
  s.setLEDFreq(30);
  s.resetBoard();
  */
  
  double t0;

  s.setUnit(Serializer::UNIT_RAW);
  s.setEncoderType(Serializer::ENCODER_QUAD);
  s.clearEncoders();

  if (1) {
    int P,I,D,L,A;
    int vpids[4], dpids[4];

    if (s.getVPID(vpids[0], vpids[1], vpids[2], vpids[3]) &&
	s.getDPID(dpids[0], dpids[1], dpids[2], dpids[3])) {
      std::cout << "Read the setting for the PIDs so that we can reset them\n";
    } else {
      std::cerr << "Could not read the setting for the PIDs, exiting\n";
      return -1;
    }

    P=10; I=0; D=5; L=10;
    if (s.setVPID(P,I,D,L)) {
      std::cout << "set VPIDs to P=" << P << " I=" << I << " D=" << D << " L=" 
		<< L << std::endl;
    } else {
      std::cerr << "Failed to set VPID parameters\n";
    }
    if (s.getVPID(P,I,D,L)) {
      std::cout << "got VPID: P=" << P << " I=" << I << " D=" << D << " L=" 
		<< L << std::endl;
    } else {
      std::cerr << "Failed to get VPID parameters\n";
    }
    P=3; I=0; D=5; L=120;
    if (s.setVPID(P,I,D,L)) {
      std::cout << "set VPIDs to P=" << P << " I=" << I << " D=" << D << " L=" 
		<< L << std::endl;
    } else {
      std::cerr << "Failed to set VPID parameters\n";
    }
    if (s.getVPID(P,I,D,L)) {
      std::cout << "VPID: P=" << P << " I=" << I << " D=" << D << " L=" 
		<< L << std::endl;
    } else {
      std::cerr << "Failed to get VPID parameters\n";
    }
    P=1; I=2; D=3; A=4;
    if (s.setDPID(P,I,D,A)) {
      std::cout << "set DPIDs to P=" << P << " I=" << I << " D=" << D << " A=" 
		<< A << std::endl;
    } else {
      std::cerr << "Failed to set DPID parameters\n";
    }
    if (s.getVPID(P,I,D,A)) {
      std::cout << "DPID: P=" << P << " I=" << I << " D=" << D << " A=" 
		<< A << std::endl;
    } else {
      std::cerr << "Failed to get DPID parameters\n";
    }
    P=3; I=0; D=5; A=120;
    if (s.setDPID(P,I,D,A)) {
      std::cout << "set DPIDs to P=" << P << " I=" << I << " D=" << D << " A=" 
		<< A << std::endl;
    } else {
      std::cerr << "Failed to set DPID parameters\n";
    }
    if (s.getVPID(P,I,D,A)) {
      std::cout << "DPID: P=" << P << " I=" << I << " D=" << D << " A=" 
		<< A << std::endl;
    } else {
      std::cerr << "Failed to get DPID parameters\n";
    }

    if (s.setVPID(vpids[0], vpids[1], vpids[2], vpids[3]) &&
	s.setDPID(dpids[0], dpids[1], dpids[2], dpids[3])) {
      std::cout << "Reset the PID values\n";
    } else {
      std::cerr << "Could not reset the PID values, bummer!!!!! They were:\n";
      std::cerr << "VPID: P=" << P << " I=" << I << " D=" << D << " L=" 
		<< L << std::endl;
      std::cerr << "DPID: P=" << P << " I=" << I << " D=" << D << " A=" 
		<< A << std::endl;
      return -1;
    }
  }

  if (0) {
    std::vector< std::pair<int,bool> > query;
    std::vector< std::pair<int,bool> > cmd;
    for (int n = 1; n <= 4; n++) {
      cmd.clear();
      for (int i = 0; i < n; i++) {
        cmd.push_back(std::make_pair(i, i%2));
      }
      std::cerr << "Setting " << n << " IOs\n";
      s.setIO(cmd);

      query.resize(n);
      for (unsigned int i = 0; i < query.size(); i++) query[i].first = i;
      t0 = s.getCurrentTime();
      std::cerr << "Getting " << n << " IOs\n";
      if (s.getIO(query)) {
	for (unsigned int i = 0; i < query.size(); i++) {
	  std::cerr << "pin" << query[i].first << "=" << query[i].second << " ";
	}
	std::cerr << std::endl;
      }
    }
  }

  if (0) {
    std::vector< std::pair<int,int> > query;
    for (int n = 1; n <= 6; n++) {
      query.resize(n);
      for (unsigned int i = 0; i < query.size(); i++) query[i].first = i;
      t0 = s.getCurrentTime();
      s.readADC(query);  
      std::cerr << "dt=" << s.getCurrentTime()-t0 << "s\n";
      for (unsigned int i = 0; i < query.size(); i++) {
        std::cerr << "c" << query[i].first << "=" << query[i].second << " ";
      }
      std::cerr << std::endl;
    }
  }

  if (0) {
    s.setMotorSpeed(0, 5);
    double t0 = s.getCurrentTime();
    while (s.getCurrentTime() - t0 < 2) {
      int speed0, speed1;
      s.getMotorSpeeds(speed0, speed1);
      std::cout << "speed0=" << speed0 << " speed1=" << speed1 << std::endl;
    }
    s.stopAllMotors();
  }

  if (0) {
    s.setMotorPWM(0, 55);
    double t0 = s.getCurrentTime();
    while (s.getCurrentTime() - t0 < 2) {
      int speed0, speed1;
      s.getMotorSpeeds(speed0, speed1);
      long enc0, enc1;
      s.getEncoders(enc0, enc1);
      std::cout << "speed0=" << speed0 << " speed1=" << speed1 
		<< " enc0=" << enc0 << " enc1=" << enc1
		<< std::endl;
    }
    s.stopAllMotors();
  }

  if (0) {
    std::vector< std::pair<int,int> > cmd;
    cmd.push_back(std::make_pair(8,10));
    cmd.push_back(std::make_pair(9,-10));
    s.setServoPos(cmd);
  }

  if (0) {
    for (int i = 0; i < 100; i++) {
      int value;
      s.readSRF08(value);
      std::cerr << "value=" << value << std::endl;
    }
  }

  s.disconnect();

  return 0;
}
